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1.  Introduction 


The  primary  motivation  for  this  work  is  to  create  a  general  framework  for  estimating  the  attitude, 
velocity,  and  position  states  for  roll-controlled,  guided  projectiles  for  which  assumptions  about 
the  projectile  dynamics  may  not  be  valid.  Guided  projectiles  could  potentially  offer  several 
strategic  advantages  to  the  Warfighter  by  reducing  the  miss  distance,  shaping  the  terminal 
trajectory,  and/or  homing  to  moving  targets.  While  guidance  solutions  exist  for  both  fin- 
stabilized  and  spin-stabilized  spinning  projectiles,  nonspinning  projectiles  are  able  to  achieve 
more  lateral  acceleration  for  conducting  more  effective  maneuvers  than  their  spinning 
counterparts  (7).  The  challenge  with  nonspinning  projectiles  is  that  they  don’t  benefit  from  the 
added  observability  provided  from  a  predictable  spin  rate.  The  “slowly  varying  spin  rate” 
assumption  is  often  used  to  correct  for  sensor  biases,  either  directly  (2,  3)  or  indirectly  ( 4 ).  As  a 
simple  example,  consider  the  output  of  a  radial  magnetometer  as  depicted  in  figure  1 .  The 
spinning  of  the  round  makes  it  fairly  easy  to  separate  a  magnetometer  sensor  bias  from  the  roll 
angle  with  respect  to  the  magnetic  field  vector  because  the  output  signal  is  periodic.  This  can 
either  be  accomplished  with  empirical  routines  such  as  peak  finding,  or  with  a  linear  state 
estimator  by  augmenting  the  projectile  state  vector  with  magnetometer  bias  states  that  are 
modeled  as  constants.  If  the  projectile  stops  rolling,  this  continuous  observability  goes  away,  and 
the  estimator  must  rely  much  more  heavily  on  the  process  models  to  account  for  the  uncertainty 
in  attitude  and  sensor  errors. 


Figure  1.  Projection  of  the  magnetic  field  vector  in  inertial  (i)  coordinates  into  the 
body-fixed  (b)  frame  as  measured  by  magnetometers  for  a  rolling 
projectile 

To  increase  the  fidelity  of  the  attitude  estimator  process  model,  the  propagation  equations  used  in 
the  prediction  step  of  the  Kalman  filtering  process  are  based  on  the  output  of  an  inertial 
measurement  unit  (IMU)  rather  than  a  projectile-specific  dynamic  model.  While  the  dynamic 
model  predictions  may  still  be  useful  as  a  heuristic  measurement,  a  dynamic  model  is  usually  an 
approximation  of  the  true  dynamics.  Errors  in  the  prediction  quality  are  usually  approximated  by 
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Gaussian  white  noise  acting  as  a  random  input  to  the  dynamic  system.  Since  this  noise  is  only  an 
artifact  of  mathematical  convenience  rather  than  a  true  physical  phenomenon,  it  cannot  be 
measured  and  is  usually  used  as  a  tuning  parameter.  When  using  kinematics  driven  by  IMU 
output  as  the  process  model,  the  statistics  of  the  gyroscope  and  accelerometer  noise  are 
measureable,  and  can  be  easily  incorporated  into  an  extended  Kalman  filtering  framework.  A 
realistic  IMU  typically  has  its  own  sources  of  modeling  error  due  to  sensor  calibration  errors, 
non-linearity,  cross-axis  sensitivity,  g-sensitivity,  misalignment,  etc.,  which  can  be  either 
modeled  or  approximated  by  increasing  the  covariance  of  the  process  noise. 

In  the  past,  both  dynamic  models  (5)  and  kinematic  models  ( 6)  have  been  used  as  process  models 
for  projectile  Kalman  filters,  as  well  as  combinations  of  the  two  (7).  There  have  been  practical 
concerns  with  using  an  IMU  on  a  projectile,  the  first  of  which  is  the  availability  of  sensors  that 
are  capable  of  measuring  the  spin  rate  of  a  projectile.  On  a  spin-stabilized  projectile  spinning  in 
the  neighborhood  of  300  Hz,  this  is  still  the  case.  However,  in  recent  years,  micro¬ 
electromechanical  system  (MEMS)  gyroscopes  have  been  able  to  measure  the  spin-rates  of  fin- 
stabilized  projectiles  such  as  mortars,  which  typically  spin  at  rates  in  the  neighborhood  of  20  Hz. 
It  is  assumed  that  any  nonspinning  projectile  will  not  be  de-spun  from  any  rate  higher  than  that 
of  typical  fin-stabilized  projectiles,  if  indeed  it  needs  to  be  de-spun  at  all.  The  other  concern  with 
using  IMUs  on  gun-launched  projectiles  is  the  tradeoff  between  affordability,  durability,  and 
performance.  Automotive-grade  MEMS  components  have  been  used  in  the  harsh  gun-launch 
environment  for  years,  and  MEMS  technology  has  generally  demonstrated  its  survivability  and 
affordability. 

There  are  still  some  challenges  with  MEMS  components,  however.  In  the  past,  sensor  bias 
changes  have  been  observed  following  the  launch  event,  and  most  inertial  sensors  either  saturate 
or  temporarily  malfunction  during  the  actual  launch  event  although  they  recover  milliseconds 
after  leaving  the  gun  barrel.  This  leads  to  an  inertial  navigation  challenge  that  is  particular  to 
gun-launched  munitions:  the  full  or  partial  loss  of  initial  conditions  immediately  following  gun 
launch.  The  final  concern  is  sensor  quality.  The  environmental  and  cost  restrictions  prohibit  the 
use  of  most  navigation  grade  or  even  tactical-grade  IMUs  used  in  other  applications.  However, 
the  flight  times  of  guided  munitions  are  very  short,  so  random  walk  due  to  relatively  poor  noise 
performance  is  not  as  much  of  a  concern  as  determining  calibration  and  initial  condition  errors 
imposed  by  gun  launch.  In  addition,  the  MEMS  industry  is  still  making  progress  in  improving 
the  noise  performance,  linearity,  temperature  stability,  cross-axis  sensitivity,  and  vibration 
rejection  of  their  products,  and  will  likely  continue  to  do  so  for  some  time.  Therefore,  at  the  very 
minimum  a  framework  should  be  in  place  for  evaluating  these  components  for  projectile  state 
estimation  as  they  improve. 

The  choice  of  attitude  representation  is  an  important  one.  Both  quaternions  and  direction  cosine 
matrices  (DCMs)  are  common  in  aerospace  applications.  Quaternions  seem  to  be  used  in  the 
majority  of  attitude  estimators,  because  they  have  fewer  elements  to  store  in  memory  than  a 
DCM,  and  they  are  easier  to  normalize.  Euler  angles  typically  don’t  see  much  use  outside  of 
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projectile  applications,  because  Euler  angles  can  become  discontinuous  when  representing 
certain  orientations  and  their  propagation  equations  can  become  indeterminate  (5).  Since  gun- 
launched  projectiles  generally  don’t  experience  a  wide  range  of  heading  angles,  this  has  not  a 
primary  concern.  The  other  major  advantage  of  quaternions  (or  DCMs)  over  Euler  angles  is  their 
propagation  equations  are  linear  with  respect  to  the  quaternion  and  only  depend  on  the  IMU’s 
angular  velocity.  This  enables  much  more  effective  use  of  the  angular  rate  sensors,  as  described 
in  section  3  of  this  report. 

A  secondary  motivation  for  writing  this  report  is  to  provide  a  detailed  explanation  of 
multiplicative  quaternion  error-state  Kalman  filter  mechanizations.  Although  this  form  of  the 
Kalman  filter  has  been  used  in  space  applications  since  the  1960s  (9)  and  many  other 
applications  (10),  clear  documentation,  especially  in  tenns  of  guided  projectiles,  has  been 
difficult  to  find  in  the  literature.  Section  2  discusses  notation,  coordinate  systems,  and  some 
preliminary  quaternion  information  that  should  make  this  report  more  readable.  Section  4 
presents  a  derivation  of  the  error  state  kinematic  equations  that  parallels  those  presented  in 
Crassidis  and  Junkins  (11)  and  Roumeliotis  et  al.  (12),  although  a  different  convention  for  the 
quaternion  definition  is  used  to  be  consistent  with  the  conventions  used  in  MATLAB  *  software 
and  Titterton  and  Weston  (8)  and  Stevens  and  Kerce  (13). 

The  position  and  velocity  error  state  kinematics  are  included  for  completeness,  although  in  a 
very  simplified  fonn.  In  the  scope  of  this  work,  the  local  Earth-fixed  navigation  system  is  treated 
as  inertial,  so  many  aspects  of  inertial  navigation  such  as  Coriolis  acceleration,  Schuler  tuning, 
and  plumb-bob  gravity  are  not  covered  here. 

Section  5  discusses  the  measurement  of  the  process  noise.  The  measurements  commonly 
available  to  guided  projectiles  and  the  mapping  of  the  error  states  to  the  residuals,  the  differences 
between  the  measured  sensor  outputs  and  predicted  ones,  are  discussed  in  section  6.  Section  7 
discusses  how  all  of  the  error  state  modeling  can  be  implemented  in  a  Kalman  filter  running  on  a 
microcontroller  with  processing  delays.  Two  different  guided  projectile  missions  are  simulated  to 
examine  the  perfonnance  of  a  Kalman  filter  that  estimates  the  attitude,  velocity,  position,  gyro 
bias,  accelerometer  bias,  and  magnetometer  bias  errors.  The  first  mission  is  a  short  range,  direct 
fire  mission  where  a  nonspinning  round  is  controlled  and  guided  to  a  moving  target.  The  second 
mission  is  a  mortar  that  is  de-spun  near  apogee  and  maneuvers  to  a  ground-based  target.  The 
simulations  and  filter  performance  are  discussed  in  section  8.  Finally,  section  9  discusses 
conclusions  and  the  direction  of  future  research. 


MATLAB  is  a  registered  trademark  of  MathWorks,  Inc.,  Natick,  MA. 
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2.  Conventions 


Vectors  are  defined  by  lowercase  bold  script.  The  terms  reference  frame  and  coordinate  system 
are  used  interchangeably,  and  all  coordinate  systems  consist  of  three  orthonormal  basis  vectors; 
for  example,  coordinate  system  a  consists  of  the  vectors  xa,ya,za .  A  superscript  denotes  the 

coordinate  system  in  which  the  coordinates  of  the  vector  are  viewed;  for  example,  v"  is  the 
coordinate  of  the  vector  v  in  the  coordinate  system  a  .  There  are  two  main  reference  frames  used 
in  this  work.  The  first  is  the  inertial  reference  (i  -frame).  A  major  simplification  used  throughout 
this  work  is  that  any  reference  frame  rigidly  fixed  to  the  surface  of  the  Earth  can  be  treated  as  an 
inertial  reference  frame  for  the  application  to  gun-launched  projectiles.  The  inertial  frame  used 
here  has  the  z-axis  pointing  toward  the  center  of  the  Earth,  the  x-axis  pointed  along  a  level  line 
from  the  gun  toward  the  initial  target  location,  and  the  y-axis  pointed  to  the  right  of  the  line  of 
fire.  The  second  coordinate  system  used  is  the  body- fixed  reference  ( b  -frame),  which  is  rigidly 
attached  to  the  projectile.  For  the  purposes  of  inertial  navigation,  it  is  assumed  that  the  origin  of 
the  body  frame  is  located  at  the  location  of  the  accelerometers  in  the  IMU.  This  prevents  the 
need  to  account  for  centripetal  and  tangential  acceleration  terms  in  the  integration  process. 
However,  the  velocity  and  position  states  obtained  will  be  those  of  the  IMU  and  not  those  of  the 
center  of  gravity  (CG)  of  the  projectile.  The  two  coordinate  systems  are  displayed  in  figure  2. 


-►xb 

Body  fixed 
coordinate  system 


X 


Target 


Z; 


Figure  2.  Coordinate  system  definitions. 
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To  relate  the  (b-frame)  to  the  (i-frame),  we  will  use  quaternion  arithmetic.  The  definition  of  a 
quaternion  used  here  has  the  scalar  component  as  the  first  element,  and  the  vector  component  as 
the  second  through  fourth  element,  i.e., 


The  rotation  vector  p  can  also  be  used  to  parameterize  the  quaternion  as  follows: 


(1) 


(2) 


The  quaternion  product,  denoted®  ,  of  any  two  quaternions  p  and  q  is  given  by  ( 14 ): 


p®q  = 


\  P\<i\  P  2:4  *  2:4 

1  ^1^2:4  ^lP2:4  +  Pi:4  X  ^2:4 


(3) 


The  quaternion  inverse  is  defined  as  the  scalar  component  with  the  negative  of  the  vector 
component: 


q 


i 


<h 

-q2:4 


(4) 


This  quaternion  inverse  multiplied  by  the  quaternion  equals  the  identity  quaternion: 


q  1  ®  q  = 


(5) 


Converting  a  vector  from  the  inertial  frame  to  the  body  frame  can  be  accomplished  with 
quaternion  rotation: 


®q . 


(6) 


The  same  rotation  can  also  be  accomplished  by  converting  the  quaternion  into  the  direction 
cosine  matrix: 

v*=C?(q)v',  (7) 


where: 
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(8) 


C;  (q)  2q2:4q2;4  +I3x3  (<7j  .0.2:4  x]  ■ 


The  general  rotation  matrix  operator  Cf  (q) reads  “transformation  from/  to b  frame  coordinates 
parameterized  by  the  quaternion  q  The  [•  x]  operator  denotes  the  skew-symmetric  matrix  of  a 

vector  v  e  □  3 ,  so  that  multiplying  a  vector  times  the  matrix  is  equivalent  to  taking  the  cross 
product  of  the  two  vectors,  i.e., 


so  that 


V2 

0  -Vj  , 

Vj  0 


(9) 


[vx  w  =  vxw. 


(10) 


3.  IMU  Integration  With  Quaternions 


Integrating  the  gyro  outputs  to  obtain  the  projectile  attitude  is  a  prerequisite  for  integrating 
acceleration  to  obtain  the  velocity  and  position  states.  The  kinematic  equations  for  a  quaternion 
are  given  by: 

(ii> 


The  vector  consists  of  the  body- fixed  coordinates  of  the  angular  velocity  of  the  body  frame 
with  respect  to  the  inertial  reference  frame.  Equation  3  can  be  used  to  reformulate  equation  1 1  as 
a  matrix  multiplication: 


o  -<r 
<4  -[<*] 

=|«(<4)q 


(12) 


It  is  clear  that  the  quaternion  propagation  equations  are  linear  with  respect  to  the  quaternion.  The 
algorithm  presented  here  is  identical  to  that  presented  in  reference  8  for  computing  the  solution 
to  equation  12  between  time  steps  in  a  sampled  system,  although  the  explanation  differs  slightly. 
For  the  remainder  of  this  section,  the  reference  frame  notation  abib  will  be  replaced  by  iteration 
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notation  to k  ,  meaning  the  value  of  (abih  sampled  at  time  A: .  In  this  case  the  solution  to  equation  12 
is  given  by: 


q*+i  =  exP 


1 


— n(w)At 

v  2 


q*- 


(13) 


The  value  of  o>  is  usually  taken  to  be: 


_  0),  +0) 
0)  =  — - - 

2 


(14) 


The  incremental  rotation  vector  can  be  approximated  as’'  o  =  (oAt .  For  a  given  rotation  vector, 
the  exact  solution  for  the  matrix  exponential  in  equation  13  is  given  by  (5): 


exp 


!«(«) 


cos 


°ll/2) 


sin 


°ll/2) 


sin 


(HA) 


I 

a  i  cos 

i 

i 


(Hl/2)1^ 


sin 


°ll/2) 


[ox 


(15) 


The  velocity  dynamics  are  a  function  of  the  specific  force  measured  by  the  accelerometers  fb ,  the 
gravity  vector  g' ,  and  the  quaternion: 

v'=C;(q)fA+g'.  (16) 


Using  equations  13-15,  it  is  possible  to  predict  what  the  quaternion  will  be  at  the  current  time 
step  based  on  angular  rate  sensor  data.  Velocity  and  position  are  estimated  via  simple  trapezoidal 
integration,  which  requires  the  quaternion  at  time  k  to  be  estimated  first.  The  velocity  update  is 
given  by: 


v,  = 


C;(q1)fI‘+Ci(qt_,)f, 


b 

k- 1 


A 


+  g 


A  t  +  y 


k-\  ' 


J 


(17) 


The  position  update  is  then  given  by: 


v»  +  v«->  Ai  +  r' 


(18) 


There  exist  higher-fidelity  representations  of  the  rotation  vector.  Bortz  (15)  derives  the  rotation  vector,  its  dynamics,  and  its 
relation  to  the  DCM  from  geometric  arguments.  It  was  shown  that  the  rotation  vector  can  be  integrated  at  a  high  integration  rate 
and  then  used  to  update  the  DCM  only  when  needed,  significantly  increasing  algorithm  efficiency.  Since  then,  particularly  in 
Savage  (16),  several  algorithms  have  been  created  for  computing  the  rotation  vector  on  digital  processors,  and  how  the  rotation 
vector  relates  to  the  DCM  and  quaternion  attitude  representation.  The  advantage  of  using  the  trapezoidal  approximation  used  here 
is  that  calibration  corrections  can  easily  be  applied  after  the  inertial  sensors  have  been  sampled  as  discussed  in  more  detail  in 
section  7,  without  needing  to  retain  several  samples  in  memory.  The  disadvantage  is  that  the  approximation  will  necessitate  a 
faster  KF  update  rate. 
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This  algorithm  assumes  that  the  specific  force  in  the  inertial  frame  is  fairly  constant  over  the 
update  interval.  The  inertial  specific  force  f'  =  C‘b  (q)f/> ,  can  be  computed  in  terms  of  the 
projectile’s  acceleration  v'  and  the  location  of  the  CG  relative  to  the  body  frame  r'’, : 

ci  (q)f *  =  -  c;  [ra  x] [<fl  x] r*  -  c;  [m  x] r‘  -  g' .  ( 1 9) 


Full  derivation  is  given  in  appendix  A. 

On  a  guided  projectile,  the  acceleration  of  the  CG  does  not  usually  change  dramatically  since 
most  projectiles  have  limited  control  authority.  Mounting  the  accelerometer  triad  on  the  spin  axis 
is  strongly  advisable,  since  centrifugal  accelerations  can  be  quite  large.  Algorithms  in  literature 
typically  don’t  use  a  trapezoidal  integration  scheme  at  all  but  use  a  combination  of  incremental 
velocity,  rotation  correction,  and  dynamic  integral  tenns,  and  potentially  more  advanced  sculling 
and  scrolling  compensation  algorithms  (8, 16).  However,  for  our  purposes  the  algorithm  provides 
a  method  of  post  applying  calibration  corrections  and  is  computationally  tractable. 


4.  Error  State  Formulation 


The  integration  of  the  quaternion,  velocity,  and  position  states  is  in  reality  driven  by  non-perfect 
gyroscopes  and  accelerometers.  Therefore,  the  integrated  states  are  only  estimations  of  the  true 
states.  Terms  that  are  estimates  are  denoted  by  the  A  over  script.  The  estimated  state  dynamics 
are  given  by: 

*“>{£}’  <20) 

t'=q(q)f‘+g',  (21) 

and  ?'  =  y' .  (22) 

From  this  point  forward,  we  will  drop  the  super  and  subscripts  for  simplicity.  The  IMU  outputs 
estimated  angular  velocity  and  specific  force  vectors,  which  are  defined  by: 

to  =  to  +  So .  (23) 

f  =  f  +  Sf  .  (24) 

The  perturbation  terms  5to  and  5f  can  be  parameterized  by  any  noise  and  sensor  calibration  errors 
substantial  enough  to  affect  the  navigation  solution. 
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The  true  quaternion  q  can  be  described  as  the  quaternion  multiplication  of  the  estimated 
quaternion  q  and  a  small  error  quaternion  Sq  : 


q  =  q  0  Sq  . 

(25) 

Sq  =  q  1  ®q  . 

(26) 

This  description  is  not  common  in  other  extended  Kalman  filtering  problems,  which  typically 
treat  the  error  terms  as  additive,  i.e.,  q  =  q  +  Sq  .  The  additive  error  assumption  is  used  in  most 
Kalman  filtering  texts  such  as  Simon  ( 17)  and  Gelb  (18).  Indeed,  there  are  even  several 
quaternion  estimators  that  use  this  assumption  (10).  Adding  two  unit  quaternions  together  does 
not  produce  another  unit  quaternion,  which  creates  a  problem  that  is  normally  dealt  with  by 
gratuitous  renonnalization.  It  is  possible,  but  quite  complicated,  to  correctly  account  for  these 
computational  aspects  in  the  error  covariance  matrix  (19).  The  multiplicative  error  formulation 
also  has  the  physical  interpretation  of  applying  a  small  rotation  correction.  This  can  easily  be 
shown  in  the  formulation  of  the  composite  DCM.  Using  the  conventions  in  this  work,  the  total 
DCM  is  given  by: 

C‘(q)  =  C(q®6q)  =  C(8q)C(q).  (27) 


Another  benefit  of  the  multiplicative  error  quaternion  is  that  only  three  terms  are  needed  to 
parameterize  it  instead  of  four.  Since  the  error  quaternion  represents  a  small  rotation,  the  rotation 
vector  p  from  equation  2  has  small  magnitude  which  means  that  the  scalar  component  of  Sq  is 
approximately  1  and  need  not  be  estimated.  The  only  downside  is  that  the  linearization  of  the 
processes  and  measurement  equations  cannot  be  accomplished  simply  by  computing  Jacobians. 

The  quaternion  error  state  dynamics  are  produced  by  differentiating  equation  26  with  respect  to 
time: 


Sq  =  q  1  0  q  +  q  1  ®  q . 


(28) 


A  substitution  for  q  1  can  be  generated  from  the  fact  that  q  1  0  q  always  produces  the  identity 
quaternion,  which  is  constant. 


(/(q  ‘®q)  .  . 

V  ■  =  q_1  0  q  +  q_1  0  q  =  0  . 


dt 


q  1  =-q  '0(10(1  1 


(29) 


(30) 
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Substituting  equations  30  and  1 1  into  equation  28  leads  to: 


Sq  =  q  1  ®~q( 

Sq  =  -^-Sq® 


f  01 

1 

KJ 

I  2< 

1®J 

®q~'®q 


f>l 

1  I 

r°l 

(ft)J 

I  2\ 

[ft)] 

(31) 


®8q 


Using  the  definition  of  the  estimated  angular  velocity  (equation  20),  this  becomes: 


1  r  o  ] 

-Sq®  ] 

2  [8(0  J 


(32) 


The  definition  of  quaternion  multiplication  can  now  be  applied  along  with  the  fact  that  8q1  =  1  to 
achieve  further  simplification: 


g^  =  ^j  -Sq2:4  (o  ]  to •  Sq2:4  ]  -Sq2:4-Sft> 

2  1  to  +  Sq2:4  x  cb J  2  ]-<o-<bx8q2:4  J  2  [Scb  +  Sq2:4  x  8d)l 


0 


0 


•  +  - 


1  f  -Sq2:4-Sft> 


(33) 


^q2:4  J  l-(ox8q2:4J  2  I  S(b  +  8q2:4  xSw  I 


Since  the  quaternion  error  states  are  assumed  to  be  small  in  magnitude,  the  error  state  dynamics 
can  be  approximated  by: 


Sq2.4  =  —to  x  8q2.4  +  ^  So . 


(34) 


At  this  point,  it  is  convenient  to  replace  the  quaternion  error  states  5q,.4  with  a  vector  of  small 
angles: 

a  =  2Sq2:4 .  (35) 

The  first  convenience  comes  from  removing  the  factor  of  Vi  from  equation  34: 

a  =  -6)xa  +  8(0.  (36) 

The  second  convenience  comes  from  the  simplification  of  equation  27.  Evaluating  equation  8 
with  Sq  and  assuming  products  of  the  terms  in  5q  ,.4  are  approximately  zero  results 

inCf  (Sq)  =  [l-2[Sq24  x]J  .  Therefore,  the  DCM  and  its  inverse  can  be  expressed  as: 


Cf(q)  =  [l-[ax]]Cf(q). 

(37) 

C;(q)  =  C;(q)[l  +  [ax]]. 

(38) 
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The  velocity  and  position  error  state  dynamics  are  simpler  to  derive,  since  their  error  states  are 
additive: 


Sv  =  v-v.  (39) 

Sr  =  r  -  r  .  (40) 

The  velocity  error  states  propagate  according  to: 

8v  =  v-v.  (41) 

By  utilizing  equations  21,  24,  38,  and  41,  an  expression  for  the  velocity  error  state  dynamics  can 
be  derived: 

5v  =  C'(q)f>+g-C;(q)fl+g5v  =  C'(q)[l  +  [ox]](f*+8f)  +  g-Ci(q)f‘-g 


8v  =  C;(q)[«x]f/’+C(q)'  8f +  C(q)^ [ax]6f +  8g 


(42) 


Due  to  the  small  angle  error  assumption,  it  is  assumed  that 

c(4)l  [l  +  [ax]]Sf  =  C(q)'  5f . 


(43) 


Also,  it  is  assumed  for  this  application  that  the  gravity  error  Sg  is  negligible.  Therefore,  equation  42 
can  be  expressed  as  a  linear  system  with  respect  to  the  error  states: 


Sv  =  -Cj, (q)  f*  x  ja  +  C(q)^  Sf . 


(44) 


The  position  error  state  dynamics  are: 

Sr  =  f-  r  =  v-  v  =  8v. 


(45) 


The  angular  rate  and  specific  force  error  states  So  and  Sf  are  parametric,  and  can  be  used  to 
model  sensor  calibration  or  noise  terms.  The  angular  rate  error  model  that  we  will  use  here  is  a 
bias  plus  noise  model: 


OJ  =  «>-Pm-1lco  (46) 

and  P*^,  (47) 

where  qo)  and  nco  are  white  random  processes  with  cross-correlations: 

^[tl<o(0TlI(T)]=J/a^(0»)5(^-'r)-  (48) 

£[nm(0^(T)]  =  ^(°pco)s(^-^)-  (49) 
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The  angular  rate  error  term  from  equations  23  and  46  becomes: 


5o>  =  -Pra-Tl<0  (5°) 

5to  =  -t)a).  (51) 

The  specific  force  measurements  from  the  accelerometers  are  treated  in  a  similar  manner: 

f  =  f-P/-T]/,  (52) 

P/=V  (53) 

and  5f  =  -p/-ii/,  (54) 

where  rj,  and  n;  are  white  random  processes  with  cross-correlations: 

E[nf  (t) x\Tf  (x)]  =  diag(a2f ) 8 (t  - x) .  (55) 

£'[t)/  (t)v^  (x)]  =  )S(t-x) .  (56) 


Magnetometers  will  be  used  as  measurements  in  the  eventual  Kalman  filter  implementation.  The 
magnetometer  is  not  an  inertial  sensor,  and  its  error  dynamics  are  uncoupled  with  the  inertial 
navigation  states,  but  a  bias  term  is  still  augmented  to  the  total  error  state  vector  since  some 
correlation  is  expected  to  be  introduced  by  the  Kalman  filter  measurement  update  equations.  The 
magnetometer  bias  vector  Pm  here  is  treated  as  a  slowly  diverging  random  walk  process  driven 
by  the  noise  process  Dm  with  cross  correlation: 

E M M]  =  diag(<*lm ) 1 § C  - 0  •  (5V) 

(58) 

The  total  error  state  vector  is  now: 

8x  =  {ar  Svr  Srr  p,/  p/  p/f.  (59) 
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The  error  state  dynamics  can  be  written  in  state  space  fonn: 


a 

-[(bx 

®3x3 

^3x3 

^3x3 

«3x3 

03x3l 

a 

-n„ 

Sv 

1  1 

X 

-Cl 

1 _ 1 

u 

1 

^3x3 

®3x3 

^3x3 

-c;(q) 

®3x3 

8v 

-C;(q)q/ 

8r 

o 

I 

o 

o 

0 

o 

5r 

03xl 

>  = 

u3x3 

*3x3 

u3x3 

u3x3 

u3x3 

u3x3 

<! 

>  +  < 

k 

^3x3 

®3x3 

®3x3 

^3x3 

«3x3 

03x3 

Pco 

^co 

P  / 

®3x3 

®3x3 

®3x3 

®3x3 

03x3 

03x3 

p  / 

k. 

^3x3 

®3x3 

®3x3 

^3x3 

03x3 

03x3! 

p«. 

.  . 

(60) 


This  18-error-state,  state-space  model  will  be  used  for  Kalman  filter  implementation  in  this 
work.  An  important  result  is  that  the  model  is  linear  and  time-varying  with  respect  to  the  error 
states.  The  advantage  of  this  is  that  when  the  Kalman  filter  is  implemented  in  realtime,  the  state 
transition  matrix  can  be  constructed  without  knowing  the  error  states  at  the  last  measurement 
time  from  the  information  available  from  the  IMU  integration.  Another  advantage  of  this  form  is 
that  there  are  no  trigonometric  operations  involved  with  its  computation.  The  third  advantage  is 
that  the  model  is  truly  a  stochastic  system  that  is  driven  by  white  noise  sequences  that  have 
spectral  densities  that  are  either  measureable  or  easily  approximated. 


5.  Process  Noise  Measurement 


From  equation  60,  the  following  matrices  are  defined: 


-[rax] 

-CUq)[f*x 

0 

0 

0 

0 


0  0  -I3x3 

0  0  0 

13X3  0  0 

0  0  0 

0  0  0 

0  0  0 


0  0 

-Ci(q)  0 

0  0 

0  0 

0  0 

0  0 


(61) 


G(q) 


diagi-a,.,) 

0 

0 

0 

0 

0 


0 

-Ch(q)diag(af) 

0 

0 

0 

0 


0 

0 

0 

0 

0 

0 


0  0  0 

0  0  0 

0  0  0 

o  0 

0  cliag  )  0 

0  0  diag[a[)m) 


(62) 
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Using  these  definitions,  the  error  state  dynamics  can  be  represented  by  the  following  matrix 
equation: 

5x  =  F5x  +  Gw.  (63) 


The  vector  w  is  a  white  noise  sequence  with  unity  variance  and  zero  mean.  The  matrix  G 
transforms  the  white  noise  sequence  into  the  disturbance  vector.  Therefore,  the  uncertainty  of  the 
disturbance  vector  can  be  represented  by: 


(Gw(t))(Gw(x))H  =  Qf5(t-x), 


(64) 


which  simplifies  to: 


Qc5(f-x)  =  GGr8(t-x)  = 


diag(cl) 

0 

0 

0 

0 

0 


0  0 

diag(<s2f)  0 


0 
0 

0  0 

0  diag(al) 

0  0 

0  0 


diag[alf) 

0 


dias(^m) 


8(f-x) .  (65) 


Representing  the  process  noise  as  a  continuous  noise  process  simplifies  filter  tuning  by  making  it 
sample  time  agnostic.  However,  discrete  time  integration  leads  to  different  variance  growth  than 
continuous  time  integration.  Consider  the  integration  of  a  scalar  random  signal.  The  integration 
of  a  continuous  white  noise  signal  with  spectral  amplitude  has  the  transfer  function: 


zkL 

w(s) 


cM 


(66) 


Therefore,  the  variance  of  the  output  y  at  some  timet  is  given  by  (17): 


E\y2  (t)]  =  |  8(u  -  v)dudv  =  <j2ct .  (67) 

Consider  the  integration  of  a  discrete  white  noise  sequence  xk  ~  N(0,od) .  If  this  sequence  is 
integrated  with  rectangular  integration,  the  output  is  given  by: 

yk=At(xk  +  xk_v..  +  x0).  (68) 
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The  output  variance  is  given  by: 


E[y2k]  =  E[At[xk  +  xk_r..  +  x0]At[xk  +xk_v..  +  x0]T] 

E[y2k  ]  =  At2  ( E\x\  ]  +  E[xU  ]  + . .  .E[x]  ])  .  (69) 

E[v2k]  =  At2ko2d  =  tAta] 


The  standard  deviation  of  the  discrete  white  noise  sequence  is  easily  estimated  from  a  short 
sample  of  a  sensor’s  null  output,  and  from  it  the  spectral  amplitude  can  be  calculated  for  an 
equivalent  continuous  noise  process.  Therefore,  in  this  work,  the  noise  amplitudes  for  the  angular 
rate  sensors  and  accelerometers,  crandoj  ,  respectively,  are  calculated  as  a  function  of  the 
estimated  discrete  time  variance  and  the  integration  sample  time: 

<3c=<3dy[At.  (70) 

The  drift-rate  terms  for  the  sensor  biases  are  not  analytically  measured  or  simulated  here.  Allan 
variance  analysis  reveals  the  existence  of  flicker  noise,  but  its  measurement,  simulation,  and 
effect  on  a  guided  projectile’s  navigation  solution  are  left  as  the  subject  of  future  work. 

Therefore,  while  the  sensor  noise  parameters  are  measured,  the  bias  drift  parameters  opc0 ,  op/ , 

and  a.,m  are  chosen  as  tuning  parameters. 


6.  Projectile  Measurement  Models 


6.1  Vector  Measurements 

Because  of  the  normal  operating  conditions  for  guided  projectiles,  there  are  only  a  few  different 
kinds  of  measurements  available.  The  most  common  are  vector  measurements  in  which  a 
vector’s  coordinates  are  either  known  a-priori  or  measured  in  both  the  inertial  and  body-fixed 
frames.  Since  magnetometers  are  probably  the  most  common  vector  measurement  used  by  the 
projectile  community,  the  Earth’s  magnetic  field  vector  m  will  be  used  to  illustrate  the  mapping 
of  the  error  states  to  a  vector’s  measurement  residuals.  Assuming  the  scale  factor  and 
misalignment  tenns  are  known,  the  magnetic  field  vector  as  measured  by  the  magnetometer  is 
given  by: 

m''=C*(q)m-+p„+il„.  (71) 

That  is,  the  measured  m/>  is  the  known  vector  in  inertial  coordinates  m'  transformed  into  the  body 
coordinates,  plus  a  body-fixed  bias  vector  Pm  and  an  additive  zero-mean  white  noise  term  x\m .  The 

£  u 

residual  5m  is  formed  by  subtracting  the  predicted  measurement  m  ’  from  the  actual  measurement. 
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(72) 


The  predicted  measurement  is  a  function  of  the  current  estimated  quaternion: 

A‘=C?(q)m'. 

Equation  37  can  be  substituted  into  equation  71  to  get: 

m6  =  [I  -  [a  x]]  C?  (q)  m'  +  p()!  +  q,„  (73) 

The  measurement  residual  is  then: 

m*  -m*  =  [i  -[ax]]C*  (q)m'  +  Pm  +  x\m  -Cbt  (q)m' ,  (74) 

which  simplifies  to  a  linear  mapping  of  the  error  states: 

5mfe=[[(cf(q)m')x]  j  +  TL-  (75) 

Another  all-weather  vector  measurement  is  the  projectile’s  velocity  vector  in  inertial 
coordinates  v' .  This  can  usually  be  obtained  from  an  onboard  GPS  unit  or  from  a  reference 
trajectory  or  dynamic  model  depending  on  the  mission  specifications.  Since  most  projectiles  fly 
at  a  fairly  low  angle  of  attack,  the  body  fixed  coordinates  are  assumed  to  be: 

v6={|v*||  0  o|r .  (76) 

The  noise  in  the  velocity  vector  is  usually  more  naturally  expressed  in  inertial  coordinates.  The 
measurement  residual  is  given  by: 

Sv'  =  v'-Ci(q)v\  (77) 

The  measured  velocity  in  inertial  coordinates  can  be  rewritten  in  tenns  of  the  error  states  and  the 
noise  vector  qv : 

v'sC:(q)[l+[«x]]v‘+i|„.  (78) 

This  leads  to  the  following  linear  mapping  of  the  error  states  to  the  velocity  vector  residual: 

Sv'  =-C;(q)[vAx]a  +  qv  (79) 

6.2  Angle  Measurements 

Some  measurement  sources  provide  direct  measurements  of  Euler  angles.  For  example,  a 
constellation  of  thennopile  sensors  with  the  appropriate  signal  processing  provides  a  direct 
measurement  of  the  roll  angle  (j)  from  a  ZYX  Euler  angle  rotation  sequence,  except  in  specific 
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weather  conditions  (2,  3).  Certain  computer  vision  algorithms  can  use  horizon  detection  to 
measure  both  the  pitch  angle  0  as  well  as  cj)  (20).  Arguably,  it  might  not  make  sense  to  even  be 
using  a  quaternion-based  attitude  representation  in  this  situation,  but  if  quaternions  are  more 
suited  for  the  mission  requirements  it  is  still  simple  to  use  Euler  angle  measurements.  In  this 
case,  one  would  compare  the  measured  Euler  angles  to  the  Euler  angles  computed  from  the 
current  quaternion  prediction  to  get  Euler  angle  error  residuals.  That  is,  for  a  typical  ZYX  Euler 
angle  rotation  sequence: 


and 

where: 


8<j>  =  <|>-<j>  +  Tl+, 

(80) 

80  =  0  -  0  +  T)0  , 

(81) 

8v  =  V-¥  +  tiv, 

(82) 

(j)  —  atan2(2(^3<?4  +<71<72)’*7i  —  ~  )  • 

(83) 

0  =  (-sin'1(2 (£2£4 -&£,))). 

(84) 

V  =  atan2 (2  (q2q3  +  qxqA ) ,  q\  +  q;  -  q, \  -  q] ) . 

(85) 

A  quaternion  Kalman  filter  estimates  small  rotation  angles  however,  so  a  mapping  must  be  used 
between  the  Euler  angle  residuals,  and  small  rotation  angle  error  states.  Assuming  that  both  Euler 
angle  residuals  and  error  states  are  small  angles,  the  mapping  between  the  two  is  identical  to  the 
mapping  between  Euler  angle  time  derivatives  and  the  body-fixed  angular  rate  vector: 


sf 
<  80  > 
8\j> 


1 


tan  0 


tan 


(e) 


0 


a . 


(86) 


0  sin  (j>  /  cos  0  cos  N)  / cos  0 


When  implementing  Euler  angle  measurements,  it  is  very  important  to  make  sure  that  the 
measured  and  predicted  quantities  are  in  the  same  quadrant. 

6.3  Position  and  Velocity  Measurements 

The  availability  and  source  of  actual  position  measurements  is  highly  subjective  to  mission 
requirements.  The  mapping  from  the  error  states  to  direct  position  or  velocity  residuals  is  simple: 

?-?  =  [l]Sr  +  r|r .  (87) 
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(88) 


v-  v  =  [l]8v  +  t]v . 


In  reality,  the  measurements  are  usually  more  complicated.  If  the  user  has  access  to  the  direct 
GPS  pseudo-range  and  carrier-frequency  measurements,  it  would  be  more  accurate  to  use  the 
well-known  GPS  measurement  models  that  include  user  clock  bias.  If  the  user  does  not  have 
access  to  the  direct  receiver  outputs  but  instead  is  provided  with  the  receiver’s  calculated 
position  and  velocity  estimate,  it  may  be  more  accurate  to  measure  the  errors  to  construct  an 
auto-regressive  or  Gauss-Markov  error  model  instead  of  assuming  white  noise.  In  the  case  of  a 
reference  trajectory,  it  may  be  beneficial  to  treat  the  dynamic  model  as  a  random  process  so  that 
the  reference  trajectory  “measurements”  get  weighed  less  and  less  as  the  trajectory  progresses  in 
time.  A  simple  way  to  do  this  is  to  model  the  errors  in  reference  trajectory  states  as  three  coupled 
position  velocity  states  driven  by  random  noise: 


0  1 
0  0 


f 

I  Sv 


+  n 


ref  * 


(89) 


Deviations  from  the  reference  trajectory  due  to  unmodeled  disturbances  such  as  wind  would  be 
accounted  for  in  the  noise  tenn  x\ref  which  is  parameterized  by  the  tuning  variable  qref : 
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8(t-r) . 


(90) 


The  measurement  covariance  at  some  time  later  can  be  calculated  from  an  initial  value  Rrt/0  and 
the  tuning  variable: 


IM'LjlM'Lj 
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t  1 
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V/3  t2/2 

r/2  t 


tief 


(91) 


This  measurement  covariance  matrix  would  then  be  used  in  the  EKF  when  calculating  the  gains 
for  the  position  and  velocity  measurements. 
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7.  Kalman  Filtering  Implementation 


The  Kalman  filter  is  eventually  intended  to  be  run  on  an  embedded  processor.  In  this  work,  this 
is  accomplished  by  discretizing  the  error  state  dynamics  so  that  the  Kalman  filtering  equations 
can  be  updated  at  a  lower  frequency  than  the  integrated  states.  The  integration  of  the  gyroscopes 
and  accelerometers  to  acquire  quaternion,  velocity,  and  position  will  be  updated  at  a  faster  rate 
than  the  EKF  since  the  integration  should  require  significantly  less  computing  power  and 
delayed  feedback  can  degrade  the  performance  of  flight  controllers.  Assuming  some  type  of 
multitasking  is  used,  the  Kalman  filter  equations  are  run  at  a  slower  rate  with  a  lower  priority  as 
shown  in  table  1 . 


Table  1.  Kalman  filter  multitasking  schedule. 


IMU  Integration  Time  i  = 

EKF  Update  Time  j  = 

0 

Start  at  X0 

1 

Measurements  y  .=0 

Kalman  Filtering 
Calculations 

Updated  X+=()  known 

Predict  X/=1 

1 

Integrate  X  from  X;.._0 

2 

Integrate  X  from  X.j 

3 

Integrate  X  from  X;_0 

4 

Integrate  X  from  x7_. 

5 

Integrate  X  from  X 

2 

Measurements  y  .=0 

Kalman  Filtering 
Calculations 

Updated  x+=1  known 
Predict  X/=2 

6 

Integrate  X  from  X;_- 

7 

Integrate  X  from  X(._6 

8 

Integrate  X  from  X;_7 

9 

Integrate  X  from  X;.._s 

— 

Etc. 

— 

Etc. 
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The  integration  of  the  full  states  is  accomplished  using  the  equations  in  section  5  and  the  best 
known  gyroscope,  and  accelerometer  calibrations.  The  actual  Kalman  filtering  calculations  are 
perfonned  to  estimate  the  error  states  at  the  time  of  the  measurements.  Each  Kalman  filter 
iteration  consists  of  the  following  five  steps: 

1 .  Measurement/Storage 

Record  the  measurements  and  their  covariance  values  (if  time  varying)  from  whatever  sources 
are  available.  Record  the  value  of  integrated  full  states  at  this  time,  as  they  will  be  used  to  form 
the  residuals. 

2.  Covariance  Prediction 


The  full  state  prediction  comes  from  the  integration  task.  The  a-priori  error-state  estimate  is  zero: 


8\j  =  0 . 


(92) 


An  estimate  of  the  a-priori  error-state  covariance  matrix  P7  must  be  predicted  from  the  previous 
iteration’s  a-posteriori  error-state  covariance  PL  and  the  error-state  dynamics.  The  matrix F 
previously  defined  in  equation  6 1  can  be  discretized  to  form  a  state  transition  matrix  using  the 
second-order  approximation  for  the  matrix  exponential: 


<F  =  I  +  FAf  +  -F2Ar . 
2 


(93) 


The  values  that  go  into  the  F  matrix  are  detennined  from  the  current  a-priori  quaternion  estimate 
and  IMU  outputs,  and  the  previous  iteration’s  a-posteriori  quaternion  estimate  and  IMU  outputs. 
The  nonzero  terms  in  F  are  therefore: 


— [ct> x]  =  —  (®._1+caJ/2x 
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-C'(q)  =  -C'(q;.,) 


(94) 


Note  that  the  term  qL  is  the  average  quaternion  detennined  from  the  algorithm  in  appendix  B,  as 
arithmetic  averaging  of  two  rotation  transfonnation  matrices  violates  the  orthonormal 
constraints.  The  a-priori  error  state  covariance  is  calculated  as: 

P:  =  E  [Sx5xr  ]  =  0>P/L0>r  +  .  (95) 

The  discrete  time  process  noise  covariance  matrix  Q ,  is  detennined  from: 


QeF  T)t/x . 


(96) 
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The  previous  equation  can  be  simplified  considerably  by  assuming  that  the  F  matrix  is  linearized 
about  the  conditions  to  =  f  =  0  andC*  (q )  =  I .  This  makes  some  sense  intuitively,  works  in 
practice,  and  is  used  in  reference  11,  but  an  analytical  reason  why  this  works  is  left  as  a  subject 
of  future  research.  With  this  simplification  and  the  definition  of  Qc  defined  in  equation  65  an 
analytical  expression  can  be  determined.  With  the  notational  shorthand  A  (v)  meaning  a  matrix 
with  the  elements  of  the  vector  v  along  the  diagonal,  the  analytical  expression  is: 
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3.  Evaluate  residual  mappings 

All  of  the  linear  maps  from  the  error  states  to  the  residuals  in  section  6  are  functions  of  the 
current  full  state  vector.  In  the  case  of  linear  Kalman  filters,  if  all  of  the  measurements  are 
uncorrelated  the  measurements  can  be  processed  one  at  a  time  with  identical  results  as 
processing  them  all  at  once  (77).  This  adds  convenience  and  reduces  computation  time  since 
matrix  inversion  is  avoided.  To  achieve  the  same  effect  with  an  EKF,  the  measurement  maps 
must  all  be  evaluated  using  the  same  a-priori  full  state  estimate. 

4.  Sequential  Kalman  filter  innovation  steps 

For  each  available  measurement  the  following  equations  steps  are  taken.  All  of  the  residual  maps 
in  section  6  can  be  put  into  the  form: 

8y  =  hSx  +  q ,  (98) 

where  8y  is  equal  to  the  actual  measurement  minus  the  predicted  measurement.  The  noise  q  will 
have  some  known  covariance  a2v .  The  Kalman  gain  is  then: 


P/h' 

(hP,V+y)' 

The  error  state  vector  is  updated  with  the  Kalman  gain: 

8x+j  =  Si)  +  kSy  . 


(99) 


(100) 
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The  error  state  covariance  matrix  is  updated  next: 

p;  =(i-kh)p;.  (101) 

If  there  are  more  measurements  to  be  processed,  then  the  a-priori  error  states  and  covariance 
matrix  are  overwritten  as 5xj  =  5x;  and  P  =  P/  before  the  innovation  steps  are  repeated  with  the 

next  measurement. 

5.  Update  the  full  states  and  calibration  parameters 

Using  the  definitions  for  each  error  state  as  defined  in  section  IV  the  full 
updated  with  the  error  state  estimate.  For  the  quaternion,  this  amounts  to 

The  position  and  velocity  estimates  are  updated  as: 


r !  =  r  +  5r  . 

(103) 

+8v . 

(104) 

The  bias  estimates  for  the  magnetometer,  gyroscopes,  and  accelerometers  are  used  to  correct  the 
calibrations  for  each  sensor,  and  then  set  to  zero.  At  this  point,  the  full  state  estimates  are 
updated  at  time  j  ,  which  will  have  been  some  distance  in  the  past  depending  on  the  speed  of  the 
processor.  A  current  full  state  estimate  can  be  obtained  by  using  the  IMU  integration  equations  in 
section  3  and  the  outputs  of  the  gyroscopes  and  accelerometers  at  time  j  and  the  current  time. 
Note  that  when  applying  equation  70  to  calculate  the  continuous  random  walk  coefficients,  better 
performance  is  usually  obtained  by  using  the  time  difference  between  the  current  time  and  time 
j  when  this  time  step  is  large  instead  of  the  integration  time  step. 


state  estimate  can  be 

(102) 


8.  Simulation  Results 


To  validate  some  of  the  models  and  algorithms  presented  here,  simulations  were  constructed  in  a 
Matlab/Simulink  environment  that  included  full  six-degree-of-freedom  (6-DOF)  motion  of  the 
projectile  as  well  as  the  state  estimation,  guidance,  and  control  algorithms.  The  simulations  are 
not  to  provide  perfonnance  metrics  for  any  specific  system,  but  to  demonstrate  the  basic 
functionality  of  the  algorithms  discussed  in  this  report  and  discuss  some  of  the  problems  with 
guided  projectile  applications. 
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8.1  Direct-Fire  Application 


The  first  system  simulation  is  a  low  QE  guide  to  hit  mission  against  a  moving  target.  The 
projectile  used  was  the  U.S.  Army/Navy  standard  finner  (21-24),  which  is  a  fin-stabilized 
nonspinning  round,  modified  with  four  canards  as  shown  in  figure  3. 


Figure  3.  U.S.  Army/Navy  standard  finner. 

The  projectile  uses  proportional  navigation  as  the  guidance  law  to  engage  the  moving  target.  The 
autopilot  is  a  state-feedback  controller  designed  using  optimal  control  techniques  (25).  The 
initial  conditions  for  the  simulation  are  shown  in  table  2. 

Table  2.  Direct- fire  initial  conditions. 


Quantity 

Value 

Units 

Muzzle  velocity 

200 

m/s 

Gun  elevation 

8 

deg 

Projectile  initial  position 

{0  0  0}T 

m 

Target  initial  position 

{1000  0  0}T 

m 

Target  velocity 

{0  10  0}T 

m/s 

Projectile  initial  angular  velocity 

{2  2  2}t 

rad/s 

The  state  estimator  is  only  an  attitude  estimator  in  this  case,  as  no  actual  position  or  velocity 
measurements  are  available.  The  gyros  are  corrupted  by  white  noise  with  the  same  parameters 
measured  from  some  modified  ADXRS300  angular  rate  sensors,  although  no  flicker  or  rate-ramp 
was  modeled.  Magnetometer  measurements  are  present  with  the  same  noise  measured  from  an 
HMC1043  solid-state  magnetometer.  It  is  assumed  that  relatively  large  initial  biases  might  be 
present  in  the  magnetometer  due  to  body- fixed  static  magnetic  fields  generated  by  the  projectile 
itself.  The  effects  of  motors  and  eddy  current  effects  are  neglected.  No  misalignment,  scale 
factor,  non-orthogonality,  or  g-sensitivity  errors  are  modeled  for  either  sensor.  In  addition  to 
magnetometers,  it  is  assumed  that  a  computer  vision  horizon  detection  algorithm  produces  roll 
and  pitch  estimates  at  30  Hz.  It  is  also  assumed  that  the  computer-vision  and  EKF  processing 
takes  80%  of  the  update  interval  (26.7  ms)  to  complete.  It  turns  out  the  yaw  angle  \|/  is 
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unobservable  with  this  configuration  (it  is  indistinguishable  from  the  x-axis  magnetometer  bias). 
To  remedy  this,  a  heuristic  yaw  angle  measurement  of  0°  is  used  for  the  first  0.5  s  to  enable  the 
sensor  biases  to  converge.  The  EKF  parameters  are  shown  in  table  3,  and  errors  are  illustrated  in 
figures  4-6. 


Table  3.  Direct-fire  EKF  parameters. 


Quantity 

Value 

Units 

Initial  attitude  error  covariance  E^aa7 l(f  =  0) 

diag{100  1  l}2 

deg2 

Initial  gyro  bias  error  covariance  T’[Pt0Pa)r  ( t  =  0) 

diagflO  10  10}2 

(deg/s)2 

Initial  magnetometer  bias  error  covariance  f'|  p,„P,„7  (t  =  0) 

diag  { 1  1  l}2 

Gauss2 

Gyro  bias  drift  covariance  diag^a^j 

diag{  1  1  l}xle-3 

(rad/s/s)2 

Magnetometer  bias  drift  covariance  diag{a2]m ) 

diag{  1  1  l}xle-3 

(Gauss/s)2 

Gyroscope  noise  std.  aad 

{  2.0626  2.0626  2.0626  }T 

deg/s 

Magnetometer  noise  std.  amd 

{0.0022  0.0022  0.0022}1 

Gauss 

Horizon  roll/pitch  measurement  noise  std. 

1 

Deg 

Quaternion  integration  rate 

500 

Hz 

Kalman  filter  update  rate 

30 

Hz 

Earth’s  magnetic  field  in  gun -target  line  coordinates 

{0.5  0  0}1 

Gauss 
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Figure  4.  Direct-fire  small-angle  errors. 
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Figure  5.  Direct-fire  gyro-bias  errors. 


Figure  6.  Direct-fire  magnetometer-bias  errors. 
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The  filtered  errors  are  not  particularly  impressive,  but  they  might  very  well  be  usable  for  this 
kind  of  mission.  The  filter  is  slightly  overconfident,  which  is  evinced  by  relatively  high  amount 
of  error  that  lies  outside  of  the  three  standard  deviation  bound  calculated  from  the  estimated  error 
covariance  matrix.  This  is  common  in  extended  Kalman  filters  because  covariance  estimates  are 
based  on  linearized  nonlinear  mapping  functions. 

8.2  Indirect-Fire  Application 

The  second  application  is  the  navigation  of  an  81 -mm  mortar  equipped  with  four  independently 
controlled  canards  that  deploy  near  apogee.  The  projectile  is  assumed  to  be  equipped 
ADXRS300  angular  rate  sensors,  a  HMC1043  solid-state  magnetometer,  and  an  ADXL278 
accelerometer  triad,  as  well  as  an  idealized  position  and  velocity  measurement  for  30  s.  The 
position  and  velocity  measurement  is  just  corrupted  with  additive  white  noise,  while  being 
available  at  100  Hz.  The  initial  conditions  of  the  trajectory  are  listed  in  table  4,  and  plot  of  the 
trajectory  is  shown  below  in  figure  7.  Before  the  canards  deploy,  the  mortar  flies  along  a  ballistic 
trajectory,  which  entails  spinning  up  to  20  Hz.  The  fact  that  real  ADXRS300  sensors  would  clip 
at  this  spin  rate  is  ignored  in  this  idealized  example. 


Table  4.  Indirect-fire  initial  conditions. 


Quantity 

Value 

Units 

Muzzle  velocity 

274 

m/s 

Gun  elevation 

45 

deg 

Projectile  initial  position 

{0  0  0}T 

m 

Target  initial  position 

{4900  100  0}T 

m 

Target  velocity 

{0  0  0}T 

m/s 

Projectile  initial  angular  velocity 

{0  4  4}t 

rad/s 

0i -  i  ^  — — - ' - i - 

0  500  1000  1500  2000  2500  3000  3500  4000  4500  5000 

x(m) 


Figure  7.  Indirect-fire  trajectory. 


26 


The  parameters  for  the  sensors  and  Kalman  filter  are  shown  in  table  5.  A  third  idealization  is  that 
the  accelerometers  are  located  at  the  projectile  center  of  gravity.  The  error  results  are  shown  in 
figures  8-13. 


Table  5.  Indirect-fire  EKF  parameters. 


Quantity 

Value 

Units 

Initial  attitude  error  covariance  ii[aar](t  =  0) 

diag{100  1  l}2 

deg2 

Initial  gyro  bias  error  covariance  £  [|f.,P,/  (/  =  0) 

diag  {10  10  10}2 

(deg/s)2 

Initial  accelerometer  bias  error  covariance  £’^p/.p/rl(t  =  0) 

diag  {10  10  10}2 

(m/s2)2 

Initial  magnetometer  bias  error  covariance  £'[pmPmr](t  =  0) 

diag{l  1  l}2 

Gauss2 

Gyro  bias  drift  covariance  diag  (tT j 

diag{l  1  l}xle-3 

(rad/s/s)2 

Accelerometer  bias  drift  covariance  diag  ( ) 

diag{l  1  l}xle-3 

(m/s7s)2 

Magnetometer  bias  drift  covariance  diag  (a^Sm ) 

diag{l  1  l}xle-3 

(Gauss/s)2 

Gyroscope  noise  std.  amd 

{  2.0626  2.0626  2.0626  }T 

deg/s 

Accelerometer  noise  std.  <sJd 

{  0.3556  0.3556  0.3556  }T 

m/s2 

Magnetometer  noise  std.  a md 

{0.0022  0.0022  0.0022}1 

Gauss 

Heuristic  yaw  measurement  noise  std. 

1 

Deg 

Heuristic  yaw  measurement  duration 

14 

s 

Position  measurement  noise  std. 

1 

m 

Position  measurement  duration 

30 

s 

Velocity  measurement  noise  std. 

1 

m/s 

Velocity  measurement  duration 

30 

s 

Velocity  vector  as  an  attitude  measurement  duration 

14 

s 

Quaternion  integration  rate 

500 

Hz 

Kalman  filter  update  rate 

100 

Hz 

Earth’s  magnetic  field  in  gun -target  line  coordinates 

{0.5  0  0}T 

Gauss 
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Figure  8.  Indirect-fire  angle  errors. 


Figure  9.  Indirect-fire  velocity  errors. 
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Figure  10.  Indirect-fire  position  errors. 
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Figure  11.  Indirect-fire  gyro-bias  errors. 
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Figure  12.  Indirect-fire  accelerometer-bias  errors. 
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Figure  13.  Indirect-fire  magnetometer-bias  errors. 
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From  the  covariance  bounds,  it  is  clear  that  some  parameters  are  more  observable  while  the 
round  is  spinning,  while  others  are  less  so.  Magnetometer  biases  and  angle  errors  are  more 
observable  before  the  canards  deploy  at  16  s.  However,  radial  accelerometer  biases  and  radial 
gyroscope  biases  have  little  to  no  observability  while  the  round  is  spinning.  This  is  because 
errors  in  these  terms  are  “rolled  out”;  that  is,  their  effects  rotate  in  direction  at  the  spin  frequency 
and  never  produce  a  noticeable  change  in  the  outputs.  Angle  errors  and  magnetometer  errors 
grow  unbounded  after  the  canards  are  deployed  and  heuristic  measurements  cease.  Interestingly, 
when  the  round  performs  a  course  correction  maneuver  at  41  s,  the  movement  provides 
information  about  the  magnetometer  bias. 


9.  Conclusions  and  Future  Work 


The  basics  of  multiplicative  quaternion  error  state  modeling  have  been  presented,  and  some 
methods  of  using  error  state  models  in  extended  Kalman  filters  have  been  discussed.  The  main 
advantage  of  using  quaternions  for  projectile  attitude  estimation  is  that  the  error  state 
propagation  equations  depend  only  on  the  estimated  gyroscope  outputs,  and  not  the  current  value 
of  the  quaternion.  This  provides  a  lot  of  flexibility  in  dealing  with  delayed  measurements  and 
processing  time  delays.  The  velocity  and  position  error  states  are  less  convenient  because  they  do 
depend  on  the  current  attitude  estimate,  but  they  provide  a  mapping  between  attitude  errors  and 
position  and  velocity  errors.  This  in  theory  provides  some  observability  into  the  attitude  errors 
from  the  information  provided  from  position  or  velocity  measurements. 

The  measurement  equations  for  common  projectile  measurements  and  heuristics  have  been 
presented,  as  well  as  the  mappings  between  error  states  and  measurement  residuals.  These 
mappings  are  linear,  but  depend  on  the  current  value  of  the  attitude  estimate.  Because  of  this, 
extended  Kalman  filters  have  problems  with  underestimating  the  error  state  covariance 
particularly  before  the  filter  has  converged,  or  when  the  system  is  not  strongly  observable. 

The  basic  functionality  of  the  error  state  modeling  has  been  shown  through  simulation  results. 
The  direct- fire  simulation  required  a  brief  heuristic  yaw  measurement  to  stabilize  the  attitude 
solution.  The  magnetometer  bias  errors,  gyroscope  bias  errors,  and  attitude  errors  were  bounded, 
but  the  filter  was  slightly  over  confident.  Nevertheless,  it  was  demonstrated  that  reasonable 
attitude  estimation  could  be  achieved  with  an  update  rate  of  only  30  Hz.  The  indirect-fire 
simulation  demonstrated  that  magnetometer  biases  are  more  observable  while  the  round  is 
spinning,  but  radial  accelerometer  and  gyroscope  biases  are  difficult  to  observe  until  the  round  is 
despun.  This  doesn’t  necessarily  prevent  someone  from  incorporating  heuristic  measurements  for 
estimating  these  biases,  but  they  are  not  implicitly  observable  through  the  error  state  modeling. 
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There  is  still  a  great  deal  of  additional  work  to  be  done  to  develop  a  functional  aided  inertial 
navigation  system  for  any  real  system.  For  example,  biases  may  not  be  the  dominant  sensor  error 
term,  and  more  involved  error  modeling  may  be  necessary.  GPS  measurements  typically  have 
their  own  error  models  that  would  need  to  be  included  in  the  filter.  There  is  also  much  room  for 
improvement  in  dealing  with  delayed  measurements.  One  improvement  for  attitude  estimators 
would  be  to  recursively  update  a  total  state  transition  matrix  at  a  higher  update  rate  than  the 
measurements,  and  use  this  for  more  accurate  error  state  and  covariance  propagation. 
Computation  time  could  be  decreased  significantly  for  the  error  state  and  covariance  prediction 
steps  due  to  matrix  partitioning,  making  this  a  viable  option  in  many  cases.  Decoupling  of  the 
attitude  and  position/velocity  states  may  lead  to  better  results  depending  on  the  update  rates  of 
the  available  measurements. 
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Given  the  location  of  the  accelerometer  w.r.t  an  inertial  frame  r’a ,  and  the  location  of  the 
projectile  center  of  gravity  (CG)  in  body  fixed  coordinates  r*  ,  the  location  of  the  CG  in  inertial 
frame  coordinates  can  be  expressed  as: 


eg 


=  r'  +C'r 

la  blcg  ' 


(A-l) 


The  output  of  the  accelerometer  will  be  proportional  to  the  acceleration  of  the  accelerometer 
relative  to  the  inertial  reference  frame,  but  the  projectile  dynamics  are  usually  formulated  relative 
to  the  CG.  The  CG  acceleration  can  be  found  by  double  differentiating  equation  A-l.  The 
position  of  the  CG  in  body  coordinates  is  assumed  to  be  constant,  and  from  here  on  to*6  will  be 
denoted  as  to : 


=  f'+q/  +Ci  rb  =r' +Ci  [tDxlr\ 

a  b  eg  b  eg  a  b  \_  J  eg 

(A-2) 

<=vl+C‘[wx]4. 

(A-3) 

=  Va+Cb  [to  x]  [to  x]rbg+  Ch  [to  x] rbg  . 

(A-4) 

The  reaction  force  of  the  restraining  “springs”  against  the  proof  mass  will  be  proportional  to  the 
accelerometer’s  acceleration  with  respect  to  the  inertial  reference  frame  minus  the  acceleration 
due  to  gravity.  The  reaction  force  divided  by  the  proof  mass  is  the  specific  force  f' . 

f!  =  -  mg' )  =  v'  -  C;  [to  x]  [to  x]  rb  -  C'  [to  x]  rb  -  g' .  (A-5) 

m 

The  output  of  the  accelerometer  is  the  specific  force  vector  resolved  in  the  body  frame: 

f ‘  =  eft'  =  C‘v;,  ~[«x][«x] r‘  - [<i> x] rj  -C'y  .  (A-6) 
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Given  an  old  quaternion  q,  and  a  new  quaternion  q2 ,  one  way  to  compute  the  average 
quaternion  q  is  to  compute  the  rotation  vector  p  that  trans forms  q,  intoq2  and  use  the  same  vector 
with  half  of  the  magnitude  to  transfonn  q,  to  q  .  If  the  quaternion  r  is  defined  byq2  =  q,  ®r  ,  then 
it  can  be  solved  for  by  r  =  q”1  <S>  q2 .  The  relationship  between  a  quaternion  and  a  rotation  vector 
can  be  used  to  solve  for  the  rotation  vector  p  : 


p  =  2  cos  1  (q ) 


(B-l) 

(B-2) 


p  =  r,  M 

2'4  sin(WI/2) 


(B-3) 


The  rotation  vector  can  be  thought  of  as  the  integration  of  and  average  angular  velocity  vector 
to  over  a  small  time  interval  At ;  p  =  toA t .  Therefore,  dividing  the  rotation  vector  by  2  has  the 
physical  interpretation  of  integrating  the  angular  velocity  for  half  the  time.  Therefore,  the  new 
rotation  quaternion  rn  is: 


cos(IH|/4) 

iSi„(H/4) 


(B-4) 


The  average  quaternion  is  thenq  =  q  ®  rn . 
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